/*************************************************************
*
* CGM_CHULL.CPP
*
* Creating a convex hull
*
* Source "Computational geometry" P.Preparata, I. Shamos p.129-131)
*************************************************************/
#include <m3d/m3d_stdafx.h>

#include <stdlib.h>
#include <string.h>

#include <ap_comm.h>
#include <m3d.h>

//
// Local defines
//
#define _Next(pV) \
   (((pV) < pointsRes + nRes - 1) ? ((pV) + 1):(pointsRes))

#define _Prev(pV) \
   ((pV) > pointsRes ) ? ((pV) - 1):(pointsRes)+(nRes)-1

#define _CGM_MAX_CHULL_VERT 100

struct _cgmCHULL_VERT {
   m2dV   v;
   float  ctg_angle;
   WORD   fAboveY;
   WORD   fBelowY;
};

BOOL cgmIsBelongPointCH_A (m2dV *pPoints, int nPoints, m2dV *v, float accuracy);

//
// Static variables
//
static _cgmCHULL_VERT  _cgmCHULLVertList[_CGM_MAX_CHULL_VERT];
static m2dV            _cgmVertList[_CGM_MAX_CHULL_VERT];

//
// Static functions
//
static int __CDECL _m3dCmpVertByAngle (const void *p1, const void *p2);

/*************************************************************
* cgmCreateConvexHull()
*
* IN: n            - number of points
*     points       - points to create a convex hull
*     pointsRes    - points of the resultant convex hull
*     pointsTmpBuf - temporary array (for internal purposes) of n points
*                    if passed as NULL, then is allocated/freed internally  
*
* OUT: number of points in convex hull
* Graham algorithm of convex hull creation. 
* See Preparata-Shamos, pp. 129-131
*************************************************************/
int cgmCreateConvexHull (int n, m2dV *points, m2dV *pointsRes, m2dV *pointsTmpBuf)
{
   int     i, nRes;
   int     iMin;
   m2dV   *pV, *pW;
   int     f;
   m2dV    pointInside;

   // if there are not more then 3 points, store them to the resultant array
   // this is a degenerate case
   if (n < 3 )  {
      memcpy(pointsRes,points,n*sizeof(m2dV));
      return n;
   }

   for (i = 2; i < n; i++ )  {
      if (m2dQualPointLine_A(&points[i],&points[0],&points[1],50*M3D_EPSILON) != M3D_LAY_ON_BORDER )  {
         break;
      }
   }

   if (i == n )  {
      // all the points are collinear
      // cannot generate the hull - degenerate case
      return -1;
   }

   if (n > _CGM_MAX_CHULL_VERT) {
      ASSERT(FALSE);
      return 0;
   }

   // interior of convex hull is not empty
   pointInside.x = (points[0].x + points[1].x + points[i].x) / 3;
   pointInside.y = (points[0].y + points[1].y + points[i].y) / 3;

   for (i = 0; i < n; i ++)  {
      m2dSubtractVector(&points[i], &pointInside, &_cgmCHULLVertList[i].v);
      _cgmCHULLVertList[i].ctg_angle = _cgmCHULLVertList[i].v.x / _cgmCHULLVertList[i].v.y;
      _cgmCHULLVertList[i].fAboveY   = _cgmCHULLVertList[i].v.y >  M3D_EPSILON;
      _cgmCHULLVertList[i].fBelowY   = _cgmCHULLVertList[i].v.y < -M3D_EPSILON;
   }
   
   // step I
   // sort points first by angle around the origin and than by 
   // distance from origin
   qsort(_cgmCHULLVertList, n, sizeof(_cgmCHULL_VERT), _m3dCmpVertByAngle);

   // remove not essential points 
   nRes = 1;
   _cgmVertList[0] = _cgmCHULLVertList[0].v;
   for (i = 0; i < n-1; i ++)  {
      if (_m3dCmpVertByAngle(&_cgmCHULLVertList[i], &_cgmCHULLVertList[i+1]) != 0) {
         // add new
         _cgmVertList[nRes++] = _cgmCHULLVertList[i+1].v;
      } else {
         if (m2dLengthVector_2(&_cgmVertList[nRes-1]) < m2dLengthVector_2(&_cgmCHULLVertList[i+1].v)) {
            // update old
            _cgmVertList[nRes-1] = _cgmCHULLVertList[i+1].v;
         }
      }
   }
   ASSERT(nRes <= n);

   // find the leftmost point
   iMin = 0;
   for (i = 0; i < nRes; i++ )  {
      if (_cgmVertList[iMin].x > _cgmVertList[i].x )  {
         iMin = i;
         continue;
      }
      if (m3dIsZero(_cgmVertList[iMin].x - _cgmVertList[i].x))  {
         if (_cgmVertList[iMin].y < _cgmVertList[i].y )  {
            iMin = i;
         }
      }
   }

   for (i = 0; i < nRes; i++ )  {
      m2dAddVector(&_cgmVertList[(iMin+i) % nRes], &pointInside, &pointsRes[i]);
   }

   // step II
   f  = 0;
   pV = pointsRes;
   pW = _Prev(pV);

   // step III
   if (nRes > 2) {
      while (!(_Next(pV) == pointsRes && f ) )  {
         if (_Next(pV) == pW )  {
            f = 1;
         }
         if (m2dQualPointLine(_Next(_Next(pV)),pV,_Next(pV)) == M3D_LAY_IN )  {
            pV = _Next(pV);
         } else {
            int t;

            t = pointsRes + nRes - _Next(pV) - 1;
            ASSERT(t >= 0);
            ASSERT(_Next(pV) + 1 + t <= pointsRes + nRes );
            if (t > 0 )  {
               memmove(_Next(pV),_Next(pV)+1,sizeof(m2dV)*t);
            }
            pW = _Prev(pW);
            nRes--;
            pV = _Prev(pV);
         }
      }
   }

   for (i = 0; i < n; i ++) {
      ASSERT(cgmIsBelongPointCH_A(pointsRes, nRes, &points[i], 0.5f));
   }
   return nRes;

#undef _Next
#undef _Prev
}

/*************************************************************
* cgmIsBelongPointCH_A ()
*
* IN:  poly  - test polygon
*      v     - test point
* RET: TRUE  - point 'v' belongs to polygon 'poly'
*      FALSE - otherwise
*
* Checks whether the 2D point belongs to the 2D polygon
* This function works for both CCW- and CW-oriented polygons
*************************************************************/
BOOL cgmIsBelongPointCH_A (m2dV *pPoints, int nPoints, m2dV *v, float accuracy)
{
   int   i;
   BOOL  fIsOrientLeft;
   int   qual;

   //ASSERT(nPoints > 2 );

   if (m2dQualPointLine_S(&pPoints[2], &pPoints[0], &pPoints[1] ,accuracy) == M3D_LAY_OUT )  {
      fIsOrientLeft = FALSE;
   } else {
      fIsOrientLeft = TRUE;
   }
   for (i = 0; i < nPoints - 1; i++ )  {
      // check that for each side the point lies inside the poly
      // formula:
      //
      // |x1 y1 1|      > 0 then (x3,y3) lies to the left of
      // |x2 y2 1| ? 0           (x1,y1)-(x2,y2)
      // |x3 y3 1|

      qual = m2dQualPointLine_S(v, &pPoints[i], &pPoints[i+1], accuracy);
      if ((qual == M3D_LAY_IN && !fIsOrientLeft) || (qual == M3D_LAY_OUT && fIsOrientLeft) )  {
         return FALSE;
      }
   }
   qual = m2dQualPointLine_S(v, &pPoints[i], &pPoints[0], accuracy);   if ((qual == M3D_LAY_IN && !fIsOrientLeft) || (qual == M3D_LAY_OUT && fIsOrientLeft) )  {
      return FALSE;
   }
   return TRUE;
} 
 

/*************************************************************
* m2dQualPointLine_S ()
*
* IN:  v          - test point
*      lB         - line segment beginning
*      lE         - line segment end
*      accuracy   - accuracy
* OUT: M3D_LAY_IN, M3D_LAY_OUT, M3D_LAY_ON_BORDER
*
* Qualify a point by a given directed line - M3D_LAY_IN (LayLeft),
* M3D_LAY_OUT (LayRight), M3D_LAY_ON_BORDER (LayOnLine)
*************************************************************/
int m2dQualPointLine_S (const m2dV *v, const m2dV *lB, const m2dV *lE, float accuracy)
{
   float dist, det;

   dist = m2dDistPointLine(v, lB, lE);
   if(dist < accuracy) {
      return M3D_LAY_ON_BORDER;
   }

   det  = lB->x*(lE->y - v->y) - lB->y*(lE->x - v->x) + lE->x*v->y - lE->y*v->x;
   if (det < 0.0f)  {
      return M3D_LAY_OUT;
   }

   if (det > 0.0f)  {
      return M3D_LAY_IN;
   }
   return M3D_LAY_ON_BORDER;
}

/*************************************************************
* Static functions
*************************************************************/
/*************************************************************
* _m3dCmpByAngle()
*
* Compare function for qsort(). Sorts points in CCW order 
* around pointInside point (static)
*************************************************************/
static int __CDECL _m3dCmpVertByAngle (const void *_p1, const void *_p2)
{
   _cgmCHULL_VERT *p1 = (_cgmCHULL_VERT*)_p1;
   _cgmCHULL_VERT *p2 = (_cgmCHULL_VERT*)_p2;

   if (p1->fBelowY && p2->fAboveY) {
      return -1;
   }
   if (p1->fAboveY && p2->fBelowY) {
      return 1;
   }
   if (m3dIsZero(p1->ctg_angle - p2->ctg_angle) )  {
      return 0;
   }
   if (p1->ctg_angle < p2->ctg_angle) {
      return 1;
   }
   // else ctg_angle1 > ctg_angle2
   return -1;
}

/*************************************************************
* _m3dSortByAngle()
*
* Compare function for qsort(). Sorts points in CCW order 
* around pointInside point (static)
*************************************************************
static int __CDECL _m3dSortByAngle_Old (const void *p1, const void *p2 )
{
   m3dV   _p1, _p2;
   float  angle1, angle2;

   _p1.x = ((m2dV*)p1)->x;
   _p1.z = ((m2dV*)p1)->y;
   _p1.y = 0.f;

   _p2.x = ((m2dV*)p2)->x;
   _p2.z = ((m2dV*)p2)->y;
   _p2.y = 0.f;
   angle1 = m3xzAngleVector(&_p1, &m3dVUnitX);
   angle2 = m3xzAngleVector(&_p2, &m3dVUnitX);

   if (m3dIsZero(angle2 - angle1) )  {
      if (m2dLengthVector((m2dV*)p1) > m2dLengthVector((m2dV*)p2))  {
         return -1;
      }
      return 1;
   }
   if (angle1 > angle2 )  {
      return 1;
   }
   return -1;
}
*/

//
// End-of-file CGM_CHULL.CPP
//


/*************************************************************
* cgmCreateConvexHull()
*
* IN: n            - number of points
*     points       - points to create a convex hull
*     pointsRes    - points of the resultant convex hull
*     pointsTmpBuf - temporary array (for internal purposes) of n points
*                    if passed as NULL, then is allocated/freed internally  
*
* OUT: number of points in convex hull
* Graham algorithm of convex hull creation. 
* See Preparata-Shamos, pp. 129-131
*************************************************************
int cgmCreateConvexHull (int n, m2dV *points, m2dV *pointsRes, m2dV *pointsTmpBuf)
{
   int     i, nRes;
   int     iMin;
   m2dV  *pV, *pW;
   int     f;
   int     fInternalBuf;
   m2dV    vOrgTo0, vOrgTo1;

   // if there are not more then 3 points, store them to the resultant array
   // this is a degenerate case
   if (n < 3 )  {
      memcpy(pointsRes,points,n*sizeof(m2dV));
      return n;
   }

   for (i = 2; i < n; i++ )  {
      if (m2dQualPointLine_A(&points[i],&points[0],&points[1],50*M3D_EPSILON) != M3D_LAY_ON_BORDER )  {
         break;
      }
   }

   if (i == n )  {
      // all the points are collinear
      // cannot generate the hull - degenerate case
      return -1;
   }

   // interior of convex hull is not empty
   _m3dSortByAngle_Org.x = (points[0].x + points[1].x + points[i].x) / 3;
   _m3dSortByAngle_Org.y = (points[0].y + points[1].y + points[i].y) / 3;

   memcpy(pointsRes, points, n*sizeof(m2dV));

   // step I
   // sort the points around the found inside point
   qsort(pointsRes, n, sizeof(m2dV), _m3dSortByAngle);

   // drop out equal points
   for (i = 0; i < n-1; i++ )  {
      if (m2dIsEqualPoint_A(&pointsRes[i], &pointsRes[i+1], 100*M3D_EPSILON)) {
         memmove(pointsRes+i, pointsRes+i+1, (n-i-1)*sizeof(m2dV));
         i--;
         n--;
      }
   }
   if (m2dIsEqualPoint_A(&pointsRes[i], &pointsRes[0], 100*M3D_EPSILON)) {
      n--;
   }

   for (i = 0; i < n-1; i++ )  {
      m2dSubtractVector(&pointsRes[i  ], &_m3dSortByAngle_Org, &vOrgTo0);
      m2dSubtractVector(&pointsRes[i+1], &_m3dSortByAngle_Org, &vOrgTo1);
      if (m3dIsZero(m2dAngleVector(&vOrgTo0, &vOrgTo1))) {
         if (m2dLengthVector_2(&vOrgTo0) > m2dLengthVector_2(&vOrgTo1)) {
            m2dV tmp;
            tmp = pointsRes[i];
            pointsRes[i] = pointsRes[i+1];
            pointsRes[i+1] = tmp;
         }
         memmove(pointsRes+i, pointsRes+i+1, (n-i-1)*sizeof(m2dV));
         i--;
         n--;
      }
   }
   m2dSubtractVector(&pointsRes[i], &_m3dSortByAngle_Org, &vOrgTo0);
   m2dSubtractVector(&pointsRes[0], &_m3dSortByAngle_Org, &vOrgTo1);
   if (m3dIsZero(m2dAngleVector(&vOrgTo0, &vOrgTo1))) {
      if (m2dLengthVector_2(&vOrgTo0) > m2dLengthVector_2(&vOrgTo1)) {
         m2dV tmp;
         tmp = pointsRes[i];
         pointsRes[i] = pointsRes[0];
         pointsRes[0] = tmp;
      }
      n--;
   }

   // find the leftmost point
   iMin = 0;
   for (i = 1; i < n; i++ )  {
      if (pointsRes[iMin].x > pointsRes[i].x )  {
         iMin = i;
         continue;
      }
      if (m3dIsZero(pointsRes[iMin].x - pointsRes[i].x))  {
         if (pointsRes[iMin].y < pointsRes[i].y )  {
            iMin = i;
         }
      }
   }

   if (pointsTmpBuf == NULL )  {
      // allocate tmp buffer internally
      pointsTmpBuf = (m2dV*)malloc(sizeof(m2dV)*n);
      ASSERT(pointsTmpBuf);
      fInternalBuf = 1;
   } else {
      fInternalBuf = 0;
   }

   memcpy(&pointsTmpBuf[n-iMin],&pointsRes[0],iMin*sizeof(m2dV));
   memcpy(&pointsTmpBuf[0],&pointsRes[iMin],(n-iMin)*sizeof(m2dV));

   memcpy(pointsRes,pointsTmpBuf,n*sizeof(m2dV));

   if (fInternalBuf ) {
      free(pointsTmpBuf);
   }

   // step II
   f = 0;
   nRes = n;
   pV = pointsRes;
   pW = _Prev(pV);

   // step III
   if (nRes > 2) {
      while (!(_Next(pV) == pointsRes && f ) )  {
         if (_Next(pV) == pW )  {
            f = 1;
         }
         if (m2dQualPointLine(_Next(_Next(pV)),pV,_Next(pV)) == M3D_LAY_IN )  {
            pV = _Next(pV);
         } else {
            int t;

            t = pointsRes + nRes - _Next(pV) - 1;
            ASSERT(t >= 0);
            ASSERT(_Next(pV) + 1 + t <= pointsRes + nRes );
            if (t > 0 )  {
               memmove(_Next(pV),_Next(pV)+1,sizeof(m2dV)*t);
            }
            pW = _Prev(pW);
            nRes--;
            pV = _Prev(pV);
         }
      }
   }
   return nRes;

#undef _Next
#undef _Prev
}*/
/*************************************************************
* _m3dSortByAngle()
*
* Compare function for qsort(). Sorts points in CCW order 
* around _m3dSortByAngle_Org point (static)
*************************************************************
static int __CDECL _m3dSortByAngle (const void *p1, const void *p2 )
{
   m3dV   _p1, _p2;
   float  angle1, angle2;

   _p1.x = ((m2dV*)p1)->x - _m3dSortByAngle_Org.x;
   _p1.z = ((m2dV*)p1)->y - _m3dSortByAngle_Org.y;
   _p1.y = 0.f;

   _p2.x = ((m2dV*)p2)->x - _m3dSortByAngle_Org.x;
   _p2.z = ((m2dV*)p2)->y - _m3dSortByAngle_Org.y;
   _p2.y = 0.f;
   angle1 = m3xzAngleVector(&_p1, &m3dVUnitZ);
   angle2 = m3xzAngleVector(&_p2, &m3dVUnitZ);

   
   if (m3dIsZero(angle2 - angle1) )  {
      if (m2dDist((m2dV*)p1, &_m3dSortByAngle_Org) > 
          m2dDist((m2dV*)p2, &_m3dSortByAngle_Org) )  {
         return -1;
      }

      return 1;
   }
   if (angle1 > angle2 )  {
      return 1;
   }
   return -1;
}
*/
